#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <wiringPi.h>
#include "car_control_pi.h"
#include "serial.h"
#define SINGLE_MSG_MAX_LEN 50
#ifdef USE_PWM
#include <softPwm.h>
#endif
static char* CTL_MOTION_T_MSG = "#%d p %d t %d";
static char* CTL_MOTION_S_MSG = "#%d p %d s %d";
static char* CTL_MOTION_GROUP_MSG = "PL %d SQ %d SM %d IX %d";
static char* CTL_MOTION_GROUP_ONCE_MSG = "PL %d SQ %d SM %d IX %d ONCE";
static char* SPACE			= " ";

static char* CTL_MSG_END = "\r\n";
static int	 END_MSG_LEN = 2;
int car_init(robot_car_pi* car)
{
	int ret = 0;
	car_wheel_cfg* wheel = car->wheel;
	car_hand_cfg* hand = car->hand;

	if (wiringPiSetup() == -1) //初始化wiringpi
	{
		printf("Setup GPIO error!\n");
		return -1;
	}

	pinMode(wheel->lw_gpioA,OUTPUT);
	pinMode(wheel->lw_gpioB,OUTPUT);
	pinMode(wheel->rw_gpioA,OUTPUT);
	pinMode(wheel->rw_gpioB,OUTPUT);
#ifdef USE_PWM
	EXC_RESULT(ret,softPwmCreate(wheel->lw_can,wheel->lw_speed,wheel->max_speed));
	EXC_RESULT(ret,softPwmCreate(wheel->rw_can,wheel->rw_speed,wheel->max_speed));
#else
	pinMode(wheel->lw_can,OUTPUT);
	pinMode(wheel->rw_can,OUTPUT);
#endif
	
	dbg_info("car_init==>lpinA:%d,lpinB:%d,rpinA:%d,rpinB:%d,lcan:%d,rcan:%d,maxspeed:%d,initspeed:%d",
		wheel->lw_gpioA,wheel->lw_gpioB,wheel->rw_gpioA,wheel->rw_gpioB,
		wheel->lw_can,wheel->rw_can,wheel->max_speed,wheel->lw_speed);

	hand->fd = init_serail(hand->dev,hand->nSpeed, hand->nBits, hand->nEvent, hand->nStop);
	hand->motor_code[0] = MOTOR_0;
	hand->motor_code[1] = MOTOR_1;
	hand->motor_code[2] = MOTOR_2;
	hand->motor_code[3] = MOTOR_3;
	hand->motor_code[4] = MOTOR_4;
	hand->motor_code[5] = MOTOR_5;
	hand->motor_code[6] = MOTOR_6;
	dbg_info("car_init==>init_serail ret fd=%d",hand->fd);
	return ret;
}

int car_move_on(robot_car_pi* car)
{
	int ret = 0;
	car_wheel_cfg* wheel = car->wheel;
	digitalWrite(wheel->lw_gpioA,GPIO_VALUE_0); //left move on
	digitalWrite(wheel->lw_gpioB,GPIO_VALUE_1);

	digitalWrite(wheel->rw_gpioA,GPIO_VALUE_0);//right move on
	digitalWrite(wheel->rw_gpioB,GPIO_VALUE_1);
#ifdef USE_PWM
	softPwmWrite(wheel->lw_can,wheel->lw_speed);
	softPwmWrite(wheel->rw_can,wheel->rw_speed);
#else
	digitalWrite(wheel->lw_can,GPIO_VALUE_1);//right move on
	digitalWrite(wheel->rw_can,GPIO_VALUE_1);
#endif

	dbg_info("car_move_on--------------");
	return ret;
}

int car_stop(robot_car_pi* car)
{
	int ret = 0;
	car_wheel_cfg* wheel = car->wheel;
	digitalWrite(wheel->lw_gpioA,GPIO_VALUE_0); //left stop
	digitalWrite(wheel->lw_gpioB,GPIO_VALUE_0);

	digitalWrite(wheel->rw_gpioA,GPIO_VALUE_0);//right stop
	digitalWrite(wheel->rw_gpioB,GPIO_VALUE_0);

#ifdef USE_PWM
	softPwmWrite(wheel->lw_can,wheel->lw_speed);
	softPwmWrite(wheel->rw_can,wheel->rw_speed);
#else
	digitalWrite(wheel->lw_can,GPIO_VALUE_0);
	digitalWrite(wheel->rw_can,GPIO_VALUE_0);
#endif
	dbg_info("car_stop--------------");
	return ret;
}

int car_back_up(robot_car_pi* car)
{
	int ret = 0;
	car_wheel_cfg* wheel = car->wheel;
	digitalWrite(wheel->lw_gpioA,GPIO_VALUE_1); //left move back
	digitalWrite(wheel->lw_gpioB,GPIO_VALUE_0);

	digitalWrite(wheel->rw_gpioA,GPIO_VALUE_1);//right move back
	digitalWrite(wheel->rw_gpioB,GPIO_VALUE_0);

#ifdef USE_PWM
	softPwmWrite(wheel->lw_can,wheel->lw_speed);
	softPwmWrite(wheel->rw_can,wheel->rw_speed);
#else
	digitalWrite(wheel->rw_can,GPIO_VALUE_1);
	digitalWrite(wheel->lw_can,GPIO_VALUE_1);
#endif
	dbg_info("car_back_up--------------");
	return ret;
}

int car_turn_left(robot_car_pi* car)
{
	int ret = 0;
	car_wheel_cfg* wheel = car->wheel;
	digitalWrite(wheel->lw_gpioA,GPIO_VALUE_1); //left move back
	digitalWrite(wheel->lw_gpioB,GPIO_VALUE_0);

	digitalWrite(wheel->rw_gpioA,GPIO_VALUE_0);//right move on
	digitalWrite(wheel->rw_gpioB,GPIO_VALUE_1);

#ifdef USE_PWM
	softPwmWrite(wheel->lw_can,wheel->lw_speed);
	softPwmWrite(wheel->rw_can,wheel->rw_speed);
#else
	digitalWrite(wheel->lw_can,GPIO_VALUE_1);
	digitalWrite(wheel->rw_can,GPIO_VALUE_1);
#endif
	dbg_info("car_turn_left--------------");
	return ret;
}

int car_turn_right(robot_car_pi* car)
{
	int ret = 0;
	car_wheel_cfg* wheel = car->wheel;
	digitalWrite(wheel->lw_gpioA,GPIO_VALUE_0); //left move on
	digitalWrite(wheel->lw_gpioB,GPIO_VALUE_1);

	digitalWrite(wheel->rw_gpioA,GPIO_VALUE_1);//right move back
	digitalWrite(wheel->rw_gpioB,GPIO_VALUE_0);

#ifdef USE_PWM
	softPwmWrite(wheel->lw_can,wheel->lw_speed);
	softPwmWrite(wheel->rw_can,wheel->rw_speed);
#else
	digitalWrite(wheel->lw_can,GPIO_VALUE_1);
	digitalWrite(wheel->rw_can,GPIO_VALUE_1);
#endif
	dbg_info("car_turn_right--------------");
	return ret;
}

int car_speed_change(robot_car_pi* car)
{
	int ret = 0;
	car_wheel_cfg* wheel = car->wheel;
#ifdef USE_PWM
	softPwmWrite(wheel->lw_can,wheel->lw_speed);
	softPwmWrite(wheel->rw_can,wheel->rw_speed);
#endif
	dbg_info("car_speed_down--------------");
	return ret;
}


//一组舵机动作接口
//多个舵机动作接口
/*
static char* CTL_MOTION_GROUP_MSG = "PL %d SQ %d SM %d IX %d";
static char* CTL_MOTION_GROUP_ONCE_MSG = "PL %d SQ %d SM %d IX %d ONCE";

int	scene_code;				//动作场景编号，只用于标识，从0开始
int	array_code;				//动作动编号0-127
int		 speed;				//执行动作速度（-200 - 200），不指定则为100，即100%一培速，负数表示逆向运行动作
int	start_code;				//指定启动动作组开始步编号，0-255
int		  loop;				//是否循环运行
*/
int car_ctl_motion_group(car_hand_cfg* cfg,motion_group_data * motion_group)
{
	int ret = 0,msg_len=0;
	char msg[SINGLE_MSG_MAX_LEN] = {0};
	if(motion_group->loop){
		msg_len= sprintf(msg,CTL_MOTION_GROUP_MSG,motion_group->scene_code,motion_group->array_code,motion_group->speed,motion_group->start_code);
	}else{
		msg_len= sprintf(msg,CTL_MOTION_GROUP_ONCE_MSG,motion_group->scene_code,motion_group->array_code,motion_group->speed,motion_group->start_code);
	}
	strcat(msg,CTL_MSG_END);
	msg_len += END_MSG_LEN;
	ret = send_data(cfg->fd,msg,msg_len);
	
	if(ret > 0){
		dbg_info("car_ctl_motion_group success:%s,len:%d",msg,ret);
		ret = SUCCESS;
	}else{
		dbg_info("car_ctl_motion_group fail:%s,ret:%d",msg,ret);
		ret = ERROR_SEARIL_SEND;
	}	
	return ret;
}

//多个舵机动作接口
/*
static char* CTL_MOTION_T_MSG = "#%d p %d t %d;
static char* CTL_MOTION_S_MSG = "#%d p %d s %d";
static char* CTL_MOTION_GROUP_MSG = "PL %d SQ %d SM %d IX %d";
static char* CTL_MOTION_GROUP_ONCE_MSG = "PL %d SQ %d SM %d IX %d ONCE";

typedef struct motion_data_{
		int motor;					//电机编号
		int postion;				//机械手电机转动到的位置
		int speed;					//机载手转动的速度
		int time;					//机械手电机转动到指定位置所需时间(单位毫秒)
}motion_data;
*/
int car_ctl_motions(car_hand_cfg* cfg,motion_data * motions,int motion_size)
{
	int ret = 0,msg_len=0,i=1;
	if(!motions || motion_size < 1){
		dbg_info("car_ctl_motions error for motinos is null");
		return ERROR_PARAM;
	}

	char* msg  = (char*)robot_malloc(motion_size*SINGLE_MSG_MAX_LEN*sizeof(char),true);
	assert(motions->motor<7 && motions->motor>=0);
	if(motions->speed > 0){
		msg_len = sprintf(msg,CTL_MOTION_S_MSG,cfg->motor_code[motions->motor],motions->postion,motions->speed);
	}else{
		msg_len = sprintf(msg,CTL_MOTION_T_MSG,cfg->motor_code[motions->motor],motions->postion,motions->time);
	}

	while(i<motion_size)
	{	
		char templet[SINGLE_MSG_MAX_LEN]={0};
		int temp_len = 0 ;
		assert((motions+i)->motor>=0 && (motions+i)->motor<7);
		if((motions+i)->speed > 0){
			temp_len = sprintf(templet,CTL_MOTION_S_MSG,cfg->motor_code[(motions+i)->motor],(motions+i)->postion,(motions+i)->speed);
		}else{
			temp_len = sprintf(templet,CTL_MOTION_T_MSG,cfg->motor_code[(motions+i)->motor],(motions+i)->postion,(motions+i)->time);
		}
		strcat(msg,SPACE);
		msg_len++;
		strcat(msg,templet);
		msg_len += temp_len;
		i++;
	}
	strcat(msg,CTL_MSG_END);
	msg_len += END_MSG_LEN;

	ret = send_data(cfg->fd,msg,msg_len);

	if(ret > 0){
		dbg_info("car_ctl_motions success:%s,len:%d",msg,ret);
		ret = SUCCESS;
	}else{
		dbg_info("car_ctl_motions fail:%s,ret:%d",msg,ret);
		ret = ERROR_SEARIL_SEND;
	}	
	
	free(msg);
	return ret;
}

int car_uninit(robot_car_pi* car)
{
	int ret = 0;
	//car_wheel_cfg* wheel = car->wheel;
	return ret;

}
